#include "IMU.h"
#include "hardwareDef.h"

#include <SPI.h>

void INIT_IMU(void)
{
  pinMode(spiCS, OUTPUT);
  digitalWrite(spiCS, HIGH);
  SPI.setDataMode(SPI_MODE0);
  SPI.setClockDivider(SPI_CLOCK_DIV4);
  SPI.setBitOrder(MSBFIRST);
  SPI.begin();
  ResetDevice();
  FIFOEnable(SC16IS752_CHANNEL_A, 1);
  FIFOEnable(SC16IS752_CHANNEL_B, 1);
  SetBaudrate(SC16IS752_CHANNEL_A, 115200);
  SetBaudrate(SC16IS752_CHANNEL_B, 115200);
  SetLine(SC16IS752_CHANNEL_A, 8, 0, 1);
  SetLine(SC16IS752_CHANNEL_B, 8, 0, 1);
}

void ResetDevice(void)
{
  uint8_t reg;
  reg = ReadRegister(SC16IS752_CHANNEL_BOTH, SC16IS750_REG_IOCONTROL);
  reg |= 0x08;
  WriteRegister(SC16IS752_CHANNEL_BOTH, SC16IS750_REG_IOCONTROL, reg);
  return;
}

void FIFOEnable(uint8_t channel, uint8_t fifo_enable)
{
  uint8_t temp_fcr = ReadRegister(channel, SC16IS750_REG_FCR);
  if (fifo_enable == 0)
  {
    temp_fcr &= 0xFE;
  }
  else
  {
    temp_fcr |= 0x01;
  }
  WriteRegister(channel, SC16IS750_REG_FCR, temp_fcr);
}

void SetBaudrate(uint8_t channel, uint32_t baudrate)
{
  uint8_t prescaler;
  if ( (ReadRegister(channel, SC16IS750_REG_MCR)&0x80) == 0)
  { //if prescaler==1
    prescaler = 1;
  }
  else
  {
    prescaler = 4;
  }
  uint16_t divisor = (SC16IS750_CRYSTCAL_FREQ/prescaler)/(baudrate*16);
  uint8_t temp_lcr = ReadRegister(channel, SC16IS750_REG_LCR);
  temp_lcr |= 0x80;
  WriteRegister(channel, SC16IS750_REG_LCR, temp_lcr);
  WriteRegister(channel, SC16IS750_REG_DLL, (uint8_t)divisor);//write to DLL
  WriteRegister(channel, SC16IS750_REG_DLH, (uint8_t)(divisor>>8));//write to DLH
  temp_lcr &= 0x7F;
  WriteRegister(channel, SC16IS750_REG_LCR, temp_lcr);
  return;
}

void SetLine(uint8_t channel, uint8_t data_length, uint8_t parity_select, uint8_t stop_length )
{
  uint8_t temp_lcr;
  temp_lcr = ReadRegister(channel, SC16IS750_REG_LCR);
  temp_lcr &= 0xC0; //Clear the lower six bit of LCR (LCR[0] to LCR[5]
  switch (data_length)
  {//data length settings
    case 5:
      break;
    case 6:
      temp_lcr |= 0x01;
      break;
    case 7:
      temp_lcr |= 0x02;
      break;
    case 8:
      temp_lcr |= 0x03;
      break;
    default:
      temp_lcr |= 0x03;
      break;
  }

  if ( stop_length == 2 )
  {
    temp_lcr |= 0x04;
  }

  //parity selection length settings
  switch (parity_select)
  {
    case 0:           //no parity
       break;
    case 1:           //odd parity
      temp_lcr |= 0x08;
      break;
    case 2:           //even parity
      temp_lcr |= 0x18;
      break;
    case 3:           //force '1' parity
      temp_lcr |= 0x03;
      break;
    case 4:           //force '0' parity
      break;
    default:
      break;
  }

  WriteRegister(channel, SC16IS750_REG_LCR,temp_lcr);
}

uint8_t ReadRegister(uint8_t channel, uint8_t reg_addr)
{
  uint8_t result;
  digitalWrite(spiCS, LOW);
  SPI.transfer(0x80|((reg_addr<<3 | channel<<1)));
  result = SPI.transfer(0xff);
  digitalWrite(spiCS, HIGH);
  return result;
}

void WriteRegister(uint8_t channel, uint8_t reg_addr, uint8_t val)
{
  digitalWrite(spiCS, LOW);
  SPI.transfer((reg_addr<<3 | channel<<1));
  SPI.transfer(val);
  digitalWrite(spiCS, HIGH);
  return;
}

int IMU_AVAILABLE(uint8_t channel)
{
  return FIFOAvailableData(channel);
}

uint8_t FIFOAvailableData(uint8_t channel)
{
  return ReadRegister(channel, SC16IS750_REG_RXLVL);
}

int IMU_READ(uint8_t channel)
{
  return ReadByte(channel);
}

void IMU_TX(uint8_t channel, uint8_t val)
{
  uint8_t tmp_lsr;
  do
  {
    tmp_lsr = ReadRegister(channel, SC16IS750_REG_LSR);
  }while ((tmp_lsr&0x20) == 0);
  WriteRegister(channel, SC16IS750_REG_THR, val);
}

int ReadByte(uint8_t channel)
{
  volatile uint8_t val;
  if (FIFOAvailableData(channel) == 0)
  {
    return -1;
  }
  else
  {
    val = ReadRegister(channel, SC16IS750_REG_RHR);
    return val;
  }
}
